{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 操作臂基本运动控制\n",
    "\n",
    "## 硬件准备\n",
    "\n",
    "材料：\n",
    "\n",
    "五自由度操作臂：F功能模块底座、I模块*2、T模块*3、G模块*1\n",
    "\n",
    "六自由度操作臂：F功能模块底座、I模块*3、T模块*3、G模块*1\n",
    "\n",
    "现在五自由度操作臂为例：\n",
    "\n",
    "## 硬件连接\n",
    "\n",
    "连接五自由度操作臂\n",
    "\n",
    "![](img/img_structure.png)\n",
    "\n",
    "## 关节配置\n",
    "\n",
    "将硬件连接的关节ID次序更新到配置文件中\n",
    "\n",
    "![](img/img_ids.png)\n",
    "\n",
    "配置完后启动设备\n",
    "\n",
    "\n",
    "# 代码说明\n",
    "\n",
    "### 导入库"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "#!/usr/bin/python\n",
    "# -*- coding: UTF-8 -*-\n",
    "import sys;\n",
    "sys.path.append(\"..\")\n",
    "sys.path.append(\"../../\")\n",
    "from lib.BwRobotLib import BwRobotLib\n",
    "from lib.MiniBotControlLib import MiniBotControlLib\n",
    "from RobotManipulator.ManipulatorRobot import ManipulatorRobot\n",
    "#from RobotManipulator.ManipulatorActions import ManipulatorActions\n",
    "import keyboard\n",
    "import time"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 声明库对象"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "#声明库对象\n",
    "robotlib = BwRobotLib()\n",
    "manipulator = ManipulatorRobot(robotlib)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 连接设备"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "连接成功\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "1"
      ]
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "#连接机器人设备\n",
    "robotlib.connectRobot(\"192.168.1.107\",55000)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 配置并初始化"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "读取ID配置文件:\n",
      "#操作臂\n",
      "\n",
      "UP:20,30,31,21,32,40\n",
      "\n",
      "#双足-双轮\n",
      "\n",
      "#UP:30,31,20\n",
      "\n",
      "#Down:32,33,21\n",
      "\n",
      "关节模块20位置：78\n",
      "关节模块30位置：80\n",
      "关节模块31位置：-87\n",
      "关节模块21位置：80\n",
      "关节模块32位置：76\n",
      "关节模块40位置：0\n"
     ]
    }
   ],
   "source": [
    "#加载配置（兼容VS项目启动方式及文件直接启动方式）\n",
    "import os,inspect\n",
    "fileName = inspect.getframeinfo(inspect.currentframe()).filename\n",
    "folderPath     = os.path.dirname(os.path.abspath(fileName))\n",
    "cfile=folderPath+\"/config.txt\"\n",
    "robotlib.loadConfig(cfile)\n",
    "\n",
    "#配置操作臂\n",
    "manipulator.bindIds(robotlib.module_ids)\n",
    "\n",
    "#读取当前坐标以初始化位置信息\n",
    "manipulator.readAngles()\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Demo示例"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "#运行\n",
    "def runDemo():\n",
    "  manipulator.pickUpSample()\n",
    "  time.sleep(2)\n",
    "  manipulator.putDownSample()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 主入口"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Enter any key to run ....\n"
     ]
    },
    {
     "name": "stdin",
     "output_type": "stream",
     "text": [
      " \n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Enter any key to run ....\n"
     ]
    }
   ],
   "source": [
    "while(1):\n",
    "  print(\"Enter any key to run ....\")\n",
    "  input(\"\")\n",
    "  runDemo()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.4"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
